#include "rviz_marker.h"

RvizMarker::RvizMarker(){
    defaultFrameId_ = "map";
    defaultScale_ = genScale(1.0, 1.0, 1.0);
    defaultPointScale_ = genScale(0.05, 0.05, 0.05);
    defaultLineScale_ = genScale(0.05, 0.0, 0.0);
}

RvizMarker::~RvizMarker(){
    
}

/***************************************************
* 生成颜色
***************************************************/
// 生成颜色
std_msgs::ColorRGBA RvizMarker::genRGBAColor(float r, float g, float b, float a){
    std_msgs::ColorRGBA color;
    color.r = r;
    color.g = g;
    color.b = b;
    color.a = a;
    return color;
}

// 生成颜色 - 蓝色
std_msgs::ColorRGBA RvizMarker::colorBlue(float a){
    return genRGBAColor(0, 0, 1.0, a);
}

// 生成颜色 - 绿色
std_msgs::ColorRGBA RvizMarker::colorGreen(float a){
    return genRGBAColor(0, 1.0, 0, a);
}

// 生成颜色 - 红色
std_msgs::ColorRGBA RvizMarker::colorRed(float a){
    return genRGBAColor(1.0, 0, 0, a);
}

// 生成颜色 - 黄色
std_msgs::ColorRGBA RvizMarker::colorYellow(float a){
    return genRGBAColor(1.0, 1.0, 0, a);
}

// 生成颜色 - 白色
std_msgs::ColorRGBA RvizMarker::colorWhite(float a){
    return genRGBAColor(1.0, 1.0, 1.0, a);
}

// 生成颜色 - 黑色
std_msgs::ColorRGBA RvizMarker::colorBlack(float a){
    return genRGBAColor(0, 0, 0, a);
}

// 生成颜色 - 灰色
std_msgs::ColorRGBA RvizMarker::colorGray(float a){
    return genRGBAColor(0.5, 0.5, 0.5, a);
}


/***************************************************
* 生成尺寸
***************************************************/
// 生成尺寸
geometry_msgs::Vector3 RvizMarker::genScale(float x, float y, float z){
    geometry_msgs::Vector3 scale;
    scale.x = x;
    scale.y = y;
    scale.z = z;
    return scale;    
}


/***************************************************
* 生成默认marker
***************************************************/  
// 点集转换
vector<geometry_msgs::Point> RvizMarker::toPoints(vector<geometry_msgs::Pose> poses){
    vector<geometry_msgs::Point> points;
    for(auto& pose : poses){
        points.emplace_back(pose.position);
    }
    return points;
}

// 四元数检测
geometry_msgs::Pose RvizMarker::checkQuaternion(geometry_msgs::Pose pose){
    if(pose.orientation.x == 0 && pose.orientation.y == 0 && pose.orientation.z == 0 && pose.orientation.w == 0){
        pose.orientation.w = 1;
    }
    return pose;
}

// 生成单位四元数
geometry_msgs::Pose RvizMarker::genDefaultPose(){
    geometry_msgs::Pose pose;
    pose.orientation.w = 1;
    return pose;
}

// 生成默认箭头Marker ARROW
visualization_msgs::Marker RvizMarker::genArrowMarker(geometry_msgs::Pose arrow_pose){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::ARROW;
    marker_msg.pose = checkQuaternion(arrow_pose);
    marker_msg.lifetime = ros::Duration(0);
    return marker_msg;
}

visualization_msgs::Marker RvizMarker::genArrowMarker(geometry_msgs::Point arrow_start, geometry_msgs::Point arrow_end){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::ARROW;
    marker_msg.pose = genDefaultPose();
    marker_msg.points.emplace_back(arrow_start);
    marker_msg.points.emplace_back(arrow_end);
    marker_msg.lifetime = ros::Duration(0);
    return marker_msg;    
}

// 生成默认立方体Marker CUBE
visualization_msgs::Marker RvizMarker::genCubeMarker(geometry_msgs::Pose center_pose, double length){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::CUBE;
    marker_msg.pose = checkQuaternion(center_pose);
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = genScale(length, length, length);
    return marker_msg;    
}

// 生成默认球体Marker SPHERE
visualization_msgs::Marker RvizMarker::genSphereMarker(geometry_msgs::Pose center_pose, double radius){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::SPHERE;
    marker_msg.pose = checkQuaternion(center_pose);
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = genScale(2.0*radius, 2.0*radius, 2.0*radius);
    return marker_msg; 
}

// 生成默认圆柱体Marker CYLINDER
visualization_msgs::Marker RvizMarker::genCylinderMarker(geometry_msgs::Pose center_pose, double radius, double length){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::CYLINDER;
    marker_msg.pose = checkQuaternion(center_pose);
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = genScale(2.0*radius, 2.0*radius, length);
    return marker_msg; 
}

// 生成默认连续线段列表Marker LINE_STRIP
visualization_msgs::Marker RvizMarker::genLineStripMarker(vector<geometry_msgs::Pose> poses){
    return genLineStripMarker(toPoints(poses));
}

visualization_msgs::Marker RvizMarker::genLineStripMarker(vector<geometry_msgs::Point> points){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::LINE_STRIP;
    marker_msg.pose = genDefaultPose();
    marker_msg.points = points;
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = defaultLineScale_;
    return marker_msg; 
}

// 生成默认独立线段列表Marker LINE_LIST
visualization_msgs::Marker RvizMarker::genLineListMarker(vector<geometry_msgs::Pose> poses){
    return genLineListMarker(toPoints(poses));
}

visualization_msgs::Marker RvizMarker::genLineListMarker(vector<geometry_msgs::Point> points){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::LINE_LIST;
    marker_msg.pose = genDefaultPose();
    marker_msg.points = points;
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = defaultLineScale_;
    return marker_msg; 
}
        
// 生成默认立方体列表Marker CUBE_LIST
visualization_msgs::Marker RvizMarker::genCubeListMarker(vector<geometry_msgs::Pose> poses, double length){
    return genCubeListMarker(toPoints(poses), length);
}

visualization_msgs::Marker RvizMarker::genCubeListMarker(vector<geometry_msgs::Point> points, double length){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
    marker_msg.pose = genDefaultPose();
    marker_msg.points = points;
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = genScale(length, length, length);
    return marker_msg; 
}
         
// 生成默认球体列表Marker SPHERE_LIST
visualization_msgs::Marker RvizMarker::genSphereListMarker(vector<geometry_msgs::Pose> poses, double radius){
    return genSphereListMarker(toPoints(poses), radius);
}

visualization_msgs::Marker RvizMarker::genSphereListMarker(vector<geometry_msgs::Point> points, double radius){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::SPHERE_LIST;
    marker_msg.pose = genDefaultPose();
    marker_msg.points = points;
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = genScale(2.0*radius, 2.0*radius, 2.0*radius);
    return marker_msg; 
}
   
// 生成默认点集列表Marker POINTS
visualization_msgs::Marker RvizMarker::genPointsMarker(vector<geometry_msgs::Pose> poses){
    return genPointsMarker(toPoints(poses));
}

visualization_msgs::Marker RvizMarker::genPointsMarker(vector<geometry_msgs::Point> points){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::POINTS;
    marker_msg.pose = genDefaultPose();
    marker_msg.points = points;
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = defaultPointScale_;
    return marker_msg; 
}
   
// 生成默认文字显示Marker TEXT_VIEW_FACING
visualization_msgs::Marker RvizMarker::genTextMarker(string text, geometry_msgs::Pose text_pose){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker_msg.pose = checkQuaternion(text_pose);
    marker_msg.text = text;
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = defaultScale_;
    return marker_msg; 
}

// 生成默认三角形列表Marker TRIANGLE_LIST
visualization_msgs::Marker RvizMarker::genTriangleListMarker(vector<geometry_msgs::Pose> poses){
    return genTriangleListMarker(toPoints(poses));
}

visualization_msgs::Marker RvizMarker::genTriangleListMarker(vector<geometry_msgs::Point> points){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::TRIANGLE_LIST;
    marker_msg.pose = genDefaultPose();
    marker_msg.points = points;
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = defaultScale_;
    return marker_msg; 
}

// 生成默认3D模型Marker MESH_RESOURCE
visualization_msgs::Marker RvizMarker::genMeshMarker(string mesh_resource, geometry_msgs::Pose mesh_pose){
    visualization_msgs::Marker marker_msg;
    marker_msg.header.frame_id = defaultFrameId_;
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.action = visualization_msgs::Marker::ADD;
    marker_msg.type = visualization_msgs::Marker::MESH_RESOURCE;
    marker_msg.pose = checkQuaternion(mesh_pose);
    marker_msg.mesh_resource = mesh_resource;
    marker_msg.lifetime = ros::Duration(0);
    marker_msg.scale = defaultScale_;
    return marker_msg;
}


/***************************************************
* 设置marker属性
***************************************************/  
// 设置坐标系、命名空间、id
void RvizMarker::setMarkerHeader(visualization_msgs::Marker& marker, string frame_id, string ns){
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;
}

// 设置ID
void RvizMarker::setMarkerId(visualization_msgs::Marker& marker, int id){
    marker.id = id;
}

// 设置尺寸
void RvizMarker::setMarkerScale(visualization_msgs::Marker& marker, geometry_msgs::Vector3 scale){
    marker.scale = scale;
}

// 设置颜色
void RvizMarker::setMarkerColor(visualization_msgs::Marker& marker, std_msgs::ColorRGBA color){
    marker.color = color;
}
